WOLF ROS2 subscribers

WOLF ROS2 subscribers are the procedures that are triggered upon receiving a message and are the main mechanism of interfacing between ROS2 and WOLF.

wolf::InterfaceBase

The abstract class wolf::InterfaceBase provides common functionality for both subscribers and publishers:

  • Stores the node pointer, giving access to the logger and the timer with the corresponding getters.

  • Stores the derived type (string).

  • Stores the topic (string).

wolf::SubscriberBase

All WOLF subscribers must inherit from the base class wolf::SubscriberBase defined in the wolf_ros2_node package.

class SubscriberBase : public InterfaceBase
{
  protected:
    SensorBasePtr sensor_ptr_;
    rclcpp::Time  last_stamp_;

  public:
    SubscriberBase(rclcpp::Node::SharedPtr _node, SensorBasePtr _sensor_ptr, const YAML::Node &_params)
        : InterfaceBase(_node, _params), sensor_ptr_(_sensor_ptr), last_stamp_(0, 0, RCL_ROS_TIME){};

    virtual ~SubscriberBase(){};

    rclcpp::Time getLastStamp() const;

    virtual double secondsSinceLastCallback();

  protected:
    void updateLastHeader(const std_msgs::msg::Header &_header);
};

This abstract class provides common functionality for all WOLF subscribers:

  • Stores a pointer to the corresponding sensor.

  • Stores the timestamp of the last received message.

  • Provides a method to update the last received message header.

The two latter are used to check messages reception in node.cpp. This requires updateLastHeader(...) to be called whitin the derived subscriber callback.

Creating a subscriber

To implement a new subscriber, you need to derive from the wolf::SubscriberBase class.

Definition

Analogously to other factory-based nodes in WOLF, the constructor should have a standard API and we provide the macro WOLF_SUBSCRIBER_CREATE to automatically implement the creator:

SubscriberDerived(rclcpp::Node::SharedPtr _node,
                  ProblemConstPtr _problem,
                  const YAML::Node& _params);
WOLF_SUBSCRIBER_CREATE(SubscriberDerived);

Also, add a ROS2 subscriber attribute and the corresponding callback in the class definition. For example:

rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr sub_pose_;
void callback(const geometry_msgs::msg::PoseStamped::SharedPtr msg);

Implementation

  1. The constructor of the derived subscriber should include:

  • Getting desired parameters.

  • Creation of the subscriber binding it to the callback method.

  1. Register the creator in the corresponding factory using the macro WOLF_REGISTER_SUBSCRIBER.

  2. Implement the callback(…) method.

    • Create a WOLF capture from the ROS message and call process(). Note that the sensor pointer is required, stored in PublisherBase::sensor_ptr_.

    • Call updateLastHeader() with the header of the message to allow message reception check.

The following is an example of a derived subscriber:

namespace wolf
{
SubscriberPose::SubscriberPose(rclcpp::Node::SharedPtr _node, SensorBasePtr _sensor_ptr, const YAML::Node &_params)
    : SubscriberBase(_node, _sensor_ptr, _params)
{
    if (not std::dynamic_pointer_cast<SensorPose2d>(_sensor_ptr) and
        not std::dynamic_pointer_cast<SensorPose3d>(_sensor_ptr))
        throw std::runtime_error("SubscriberPose: sensor provided should be of type SensorPose2d or SensorPose3d!");

    sub_pose_ = _node->create_subscription<geometry_msgs::msg::PoseStamped>(
        getTopic(), 100, std::bind(&SubscriberPose::callback, this, std::placeholders::_1));
}

void SubscriberPose::callback(const geometry_msgs::msg::PoseStamped::SharedPtr msg)
{
    updateLastHeader(msg->header);

    Eigen::Vector7d data;
    data << msg->pose.position.x, msg->pose.position.y, msg->pose.position.z, msg->pose.orientation.x,
        msg->pose.orientation.y, msg->pose.orientation.z, msg->pose.orientation.w;
    CapturePosePtr new_capture =
        std::make_shared<CapturePose>(TimeStamp(msg->header.stamp.sec, msg->header.stamp.nanosec),
                                      sensor_ptr_,
                                      data,
                                      sensor_ptr_->computeNoiseCov(data));
    sensor_ptr_->process(new_capture);
}

WOLF_REGISTER_SUBSCRIBER(SubscriberPose)
}  // namespace wolf

Specification

You will have to create the corresponding schema file specifying the YAML parameters. The following is an example of a derived subscriber schema:

follow: SubscriberBase.schema

inverse_detections:
  _type: bool
  _mandatory: true
  _doc: If the detections are in landmark frame coordinates

type_offset:
  _type: unsigned int
  _mandatory: false
  _default: 0
  _doc: If an offset has to be applied on the types (useful when there are different classifiers working at the same time)

id_offset:
  _type: unsigned int
  _mandatory: false
  _default: 0
  _doc: If an offset has to be applied on the ids (useful when there are different trackers working at the same time)

sensor_extrinsics:
  _type: VectorXd
  _mandatory: false
  _default: [0, 0, 0, 0, 0, 0, 1]
  _doc: The 3D transformation of the sensor to be applied a priori (in the correponding sensor there should be zero extrinsics if defined here. Useful when 3D detections in 2D problems)

Finally, remember to add the new subscriber to the CMakeLists.txt of the correponding package.